package pl.polsl.pum2.pj.math;

public class Quaternion3d {

  private float              x;
  private float              y;
  private float              z;
  private float              w;

  private static final float QUATERNION_TRACE_ZERO_TOLERANCE = 0.1f;

  private Quaternion3d() {}

  public Quaternion3d(float x, float y, float z, float w) {
    this.x = x;
    this.y = y;
    this.z = z;
    this.w = w;
  }

  public Quaternion3d clone() {
    return new Quaternion3d(x, y, z, w);
  }

  public static Quaternion3d identity() {
    return new Quaternion3d(0, 0, 0, 1);
  }

  public static void normalize(Quaternion3d quaternion) {
    float magnitude = (float) Math
        .sqrt(((quaternion.x * quaternion.x) + (quaternion.y * quaternion.y) + (quaternion.z * quaternion.z) + (quaternion.w * quaternion.w)));

    if (magnitude != 0) {
      quaternion.x /= magnitude;
      quaternion.y /= magnitude;
      quaternion.z /= magnitude;
      quaternion.w /= magnitude;
    }
  }

  public static Quaternion3d fromMatrix(float[] matrix) {
    Quaternion3d quat = new Quaternion3d();

    // Trace of diagonal
    float trace = matrix[0] + matrix[5] + matrix[10];

    if (trace > 0.0f) {
      float s = (float) Math.sqrt(trace + 1.0f);
      quat.w = s * 0.5f;
      s = 0.5f / s;

      quat.x = (matrix[9] - matrix[6]) * s;
      quat.y = (matrix[2] - matrix[8]) * s;
      quat.z = (matrix[4] - matrix[1]) * s;

      return quat;
    }

    if (matrix[0] > matrix[5]) {
      if (matrix[10] > matrix[0]) {
        if (!option2(matrix, quat)) {
          if (!option1(matrix, quat)) {
            option3(matrix, quat);
          }
        }
      } else {
        if (!option1(matrix, quat)) {
          if (!option2(matrix, quat)) {
            option3(matrix, quat);
          }
        }
      }
    } else {
      if (!option3(matrix, quat)) {
        if (!option2(matrix, quat)) {
          option1(matrix, quat);
        }
      }
    }
    return quat;
  }

  private static boolean option1(float[] matrix, Quaternion3d quat) {
    float s = (float) Math.sqrt(matrix[0] - (matrix[5] + matrix[10]) + 1.0f);
    if (s > QUATERNION_TRACE_ZERO_TOLERANCE) {
      quat.x = s * 0.5f;
      s = 0.5f / s;
      quat.w = (matrix[9] - matrix[6]) * s;
      quat.y = (matrix[1] + matrix[4]) * s;
      quat.z = (matrix[2] + matrix[8]) * s;
      return true;
    }
    return false;
  }

  private static boolean option2(float[] matrix, Quaternion3d quat) {
    float s = (float) Math.sqrt(matrix[10] - (matrix[0] + matrix[5]) + 1.0f);
    if (s > QUATERNION_TRACE_ZERO_TOLERANCE) {
      quat.z = s * 0.5f;
      s = 0.5f / s;
      quat.w = (matrix[4] - matrix[1]) * s;
      quat.x = (matrix[8] + matrix[2]) * s;
      quat.y = (matrix[9] + matrix[6]) * s;
      return true;
    }
    return false;
  }

  private static boolean option3(float[] matrix, Quaternion3d quat) {
    float s = (float) Math.sqrt(matrix[5] - (matrix[10] + matrix[0]) + 1.0f);
    if (s > QUATERNION_TRACE_ZERO_TOLERANCE) {
      quat.y = s * 0.5f;
      s = 0.5f / s;
      quat.w = (matrix[2] - matrix[8]) * s;
      quat.z = (matrix[6] + matrix[9]) * s;
      quat.x = (matrix[4] + matrix[1]) * s;
      return true;
    }
    return false;
  }

  public static void setMatrixFromQuaternion(float[] matrix, Quaternion3d quat) {
    matrix[0] = 1.0f - (2.0f * ((quat.y * quat.y) + (quat.z * quat.z)));
    matrix[1] = 2.0f * ((quat.x * quat.y) - (quat.z * quat.w));
    matrix[2] = 2.0f * ((quat.x * quat.z) + (quat.y * quat.w));
    matrix[3] = 0.0f;
    matrix[4] = 2.0f * ((quat.x * quat.y) + (quat.z * quat.w));
    matrix[5] = 1.0f - (2.0f * ((quat.x * quat.x) + (quat.z * quat.z)));
    matrix[6] = 2.0f * ((quat.y * quat.z) - (quat.x * quat.w));
    matrix[7] = 0.0f;
    matrix[8] = 2.0f * ((quat.x * quat.z) - (quat.y * quat.w));
    matrix[9] = 2.0f * ((quat.y * quat.z) + (quat.x * quat.w));
    matrix[10] = 1.0f - (2.0f * ((quat.x * quat.x) + (quat.y * quat.y)));
    matrix[11] = 0.0f;
    matrix[12] = 0.0f;
    matrix[13] = 0.0f;
    matrix[14] = 0.0f;
    matrix[15] = 1.0f;
  }

  public static Quaternion3d fromAxisAndAngle(Vector3d axis, float angle) {
    Quaternion3d quat = new Quaternion3d();

    angle *= 0.5f;
    Vector3d.normalize(axis);
    float sinAngle = (float) Math.sin(angle);
    quat.x = (axis.x * sinAngle);
    quat.y = (axis.y * sinAngle);
    quat.z = (axis.z * sinAngle);
    quat.w = (float) Math.cos(angle);

    return quat;
  }

  public static float extractAxisAndAngle(Quaternion3d quat, Vector3d axis) {
    normalize(quat);
    float s = (float) Math.sqrt(1.0f - (quat.w * quat.w));
    if (Math.abs(s) < 0.0005f) {
      s = 1.0f;
    }

    if (axis != null) {
      axis.x = (quat.x / s);
      axis.y = (quat.y / s);
      axis.z = (quat.z / s);
    }

    return (float) (Math.acos(quat.w) * 2.0f); // return angle as float
  }

  public static Quaternion3d multiply(Quaternion3d quat1, Quaternion3d quat2) {
    Vector3d v1 = new Vector3d(quat1.x, quat1.y, quat1.z);
    Vector3d v2 = new Vector3d(quat2.x, quat2.y, quat2.z);

    float angle = (quat1.w * quat2.w) - Vector3d.dotProduct(v1, v2);

    Vector3d cp = Vector3d.crossProduct(v1, v2);

    v1.x *= quat2.w;
    v1.y *= quat2.w;
    v1.z *= quat2.w;

    v2.x *= quat1.w;
    v2.y *= quat1.w;
    v2.z *= quat1.w;

    return new Quaternion3d(v1.x + v2.x + cp.x, v1.y + v2.y + cp.y, v1.z + v2.z + cp.z, angle);
  }

  public static void invert(Quaternion3d quat) {
    float length = 1.0f / ((quat.x * quat.x) + (quat.y * quat.y) + (quat.z * quat.z) + (quat.w * quat.w));
    quat.x *= -length;
    quat.y *= -length;
    quat.z *= -length;
    quat.w *= length;
  }

  public static Quaternion3d fromEulerAngles(float x, float y, float z) {
    Vector3d vx = new Vector3d(1.f, 0.f, 0.f);
    Vector3d vy = new Vector3d(0.f, 1.f, 0.f);
    Vector3d vz = new Vector3d(0.f, 0.f, 1.f);

    Quaternion3d qx = fromAxisAndAngle(vx, x);
    Quaternion3d qy = fromAxisAndAngle(vy, y);
    Quaternion3d qz = fromAxisAndAngle(vz, z);

    Quaternion3d temp = multiply(qx, qy);
    return multiply(temp, qz);
  }

  public static float dotProduct(Quaternion3d quat1, Quaternion3d quat2) {
    return quat1.x * quat2.x + quat2.y * quat2.y + quat1.z * quat2.z + quat1.w * quat2.w;
  }

  public static Quaternion3d slerp(Quaternion3d start, Quaternion3d finish, float progress) {
    float startWeight, finishWeight;
    float difference = (start.x * finish.x) + (start.y * finish.y) + (start.z * finish.z) + (start.w * finish.w);
    if (1f - Math.abs(difference) > .01f) {
      float theta = (float) Math.acos(Math.abs(difference));
      float oneOverSinTheta = (float) (1.f / Math.sin(theta));
      startWeight = (float) (Math.sin(theta * (1.f - progress)) * oneOverSinTheta);
      finishWeight = (float) (Math.sin(theta * progress) * oneOverSinTheta);
      if (difference < 0f) {
        startWeight = -startWeight;
      }
    } else {
      startWeight = (1.f - progress);
      finishWeight = progress;
    }

    Quaternion3d ret = new Quaternion3d();
    ret.x = (start.x * startWeight) + (finish.x * finishWeight);
    ret.y = (start.y * startWeight) + (finish.y * finishWeight);
    ret.z = (start.z * startWeight) + (finish.z * finishWeight);
    ret.w = (start.w * startWeight) + (finish.w * finishWeight);
    normalize(ret);
    return ret;
  }
}
